import rclpy
import rclpy.time
from rclpy.node import Node

# from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import PoseStamped, Pose
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult # 判断结果的类

# TF坐标监听器
from tf2_ros import TransformListener, Buffer
from geometry_msgs.msg import TransformStamped # TF消息接口
from tf_transformations import euler_from_quaternion, quaternion_from_euler # 四元数转欧拉角
# ROS_MSG使用四元数记录欧拉角信息
import math # 角度转弧度

from autopatrol_interfaces.srv import SpeechText

# 接入相机图像识别模块
from sensor_msgs.msg import Image # 图像消息接口
from cv_bridge import CvBridge # ROS图像消息与OpenCV图像格式转换器
import cv2 # OpenCV图像处理库(读取和保存图像)

# 继承BasicNavigator类(里面自带Node类)
class PatrolNode(BasicNavigator):
    def __init__(self, node_name='patrol_node'):
        super().__init__(node_name) # 调用父类的初始化函数
        self.buffer_ = Buffer() # 创建TF缓存器
        self.dynamic_listener_ = TransformListener(self.buffer_, self)
        # 声明可变参数
        self.declare_parameter('initial_point', [0.0, 0.0, 0.0])
        # 此处错误,参数不支持二维数组
        # self.declare_parameter('target_points', [[1.0, 1.0, 0.0], [2.0, 2.0, 0.0]]) # 目标点列表
        self.declare_parameter('target_points', [1.0, 1.0, 0.0, 2.0, 2.0, 0.0]) # 目标点列表
        self.initial_point_ = self.get_parameter('initial_point').value
        self.target_points_ = self.get_parameter('target_points').value
        # 语音服务器客户端
        self.speech_client_ = self.create_client(SpeechText, 'speech_text_service')
        # 图像消息与OpenCV图像格式转换器
        self.declare_parameter('img_save_path','') # 保存图像路径,默认为空,表示保存在这个目录下
        self.img_save_path_ = self.get_parameter('img_save_path').value
        self.bridge_ = CvBridge() # 创建图像消息与OpenCV图像格式转换器对象
        self.latest_img_ = None # 最新图像
        self.img_sub_ = self.create_subscription(Image, '/camera_sensor/image_raw', self.img_callback, 1) # 创建图像订阅者 ,z最后一个为数据缓存队列长度,只保存一个

    def img_callback(self, msg):
        """
        图像消息回调函数
        """
        self.latest_img_ = msg # 保存最新图像

    def record_img(self):
        """
        保存图像
        """
        if self.latest_img_ is not None:
            pose = self.get_robot_pose() # 获取机器人当前位姿
            cv_img = self.bridge_.imgmsg_to_cv2(self.latest_img_) # 图像消息转换为OpenCV图像格式
            cv2.imwrite(f"{self.img_save_path_}img_{pose.translation.x:3.2f}_{pose.translation.y:3.2f}.png", cv_img) # 保存图像

    def get_pose_by_xyyaw(self, x, y, yaw):
        """
        根据x, y, yaw计算出PoseStamped消息,返回RoseStamped消息对象
        """
        pose = PoseStamped()
        pose.header.frame_id = "map"
        pose.header.stamp = self.get_clock().now().to_msg()
        pose.pose.position.x = x
        pose.pose.position.y = y
        q = quaternion_from_euler(0, 0, yaw)
        pose.pose.orientation.x = q[0]
        pose.pose.orientation.y = q[1]
        pose.pose.orientation.z = q[2]
        pose.pose.orientation.w = q[3]
        return pose

    def init_robot_pose(self):
        """
        初始化机器人位姿
        """
        self.initial_point_ = self.get_parameter('initial_point').value
        init_pose = self.get_pose_by_xyyaw(self.initial_point_[0], self.initial_point_[1], self.initial_point_[2])
        self.setInitialPose(init_pose)
        self.waitUntilNav2Active() # Wait for Nav2 to be ready

    def get_target_points(self):
        """
        获取目标点列表
        """
        points = []
        self.target_points_ = self.get_parameter('target_points').value
        for index in range(int(len(self.target_points_)/3)): # 双重列表,遍历外围列表
            x = self.target_points_[index*3]   
            y = self.target_points_[index*3+1]
            yaw = self.target_points_[index*3+2]
            points.append([x, y, yaw])
            self.get_logger().info(f"目标点{index}: {x}, {y}, {yaw}")
        return points

    def nav_to_pose(self, target_point):
        """
        导航到目标点
        """
        self.get_logger().info("开始导航")
        self.goToPose(target_point)
    
        # 等待导航完成
        while not self.isTaskComplete():
            feedback = self.getFeedback()
            self.get_logger().info(f"Feedback: 剩余距离: {feedback.distance_remaining}")
            # 超时取消函数 navigator.cancelTask()
        result = self.getResult()
        self.get_logger().info(f"Result: {result}")

    def get_robot_pose(self):
        """
        获取机器人当前位姿
        """
        while rclpy.ok():
            """ 获取TF坐标 """
            try:
                result = self.buffer_.lookup_transform("map", "base_footprint", 
                    rclpy.time.Time(seconds=0.0), 
                    rclpy.time.Duration(seconds=1.0)) # 获取TF坐标
                transform = result.transform # 获取TF坐标
                self.get_logger().info(f"平移:{transform.translation}") # 打印日志信息
                self.get_logger().info(f"旋转四元数:{transform.rotation}") # 打印日志信息
                rotation = euler_from_quaternion([transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w]) # 四元数转欧拉角
                self.get_logger().info(f"旋转欧拉角:{rotation}") # 打印日志信息
                return transform # 返回TF坐标
            except Exception as e:
                self.get_logger().error(f"TF坐标获取失败:{str(e)}") # 打印日志信息

    def speech_text(self, text):
        """
        语音播报
        """
        self.get_logger().info(f"播报:{text}") # 打印日志信息
        while not self.speech_client_.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('语音播报服务器未就绪,等待1s...')
        request = SpeechText.Request()
        request.text = text
        future = self.speech_client_.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        # 处理服务器返回结果
        if future.result() is not None:
            response = future.result()
            if response.result == True:
                self.get_logger().info('语音播报成功')
            else:
                self.get_logger().warn('语音播报失败')
        else:
            self.get_logger().error('语音播报服务器未响应')


def main():
    rclpy.init()
    patrol = PatrolNode()
    patrol.speech_text("正在准备初始化位置")
    # rclpy.spin(patrol)
    # 初始化机器人位姿
    patrol.init_robot_pose()
    patrol.speech_text("位置初始化完成")

    while rclpy.ok():
        # 获取目标点列表
        target_points = patrol.get_target_points()
        # 遍历目标点列表
        for target_point in target_points:
            x, y, yaw = target_point[0], target_point[1], target_point[2]
            # 目标点转换为PoseStamped消息
            target_pose = patrol.get_pose_by_xyyaw(x, y, yaw)
            patrol.speech_text(f"正在准备前往{x}, {y}目标点")
            # 导航到目标点
            patrol.nav_to_pose(target_pose)
            patrol.speech_text(f"已经到达{x}, {y}目标点.,准备拍照记录图片")
            # 保存图像
            patrol.record_img()
            patrol.speech_text(f"已保存图像,准备前往下一个目标点")
            # 等待机器人位姿更新


    # rclpy.spin(navigator)
    rclpy.shutdown()